package com.gitee.wsl.mathematics.complex.quaternion.ext

import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.mathematics.vector.vec4.Vector4

fun<T:Number,Q: Quaternion<T, Q>> Quaternion<T, Q>.transform(v: Vector3<T, *>): Vector3<T, *>{
    // Create a pure quaternion from the vector
    val q = this
    val p = create(v.x, v.y, v.z, zero)
    // Multiply q by p, then by the conjugate of q
    val resultQuaternion = (q * p) * q.conjugate
    return v.create(resultQuaternion.x, resultQuaternion.y, resultQuaternion.z) as Vector3<T, *>
}

fun<T:Number,Q: Quaternion<T, Q>> Quaternion<T, Q>.transform(v: Vector4<T, *>): Vector4<T, *>{
    // calculate quat * vec
    val ix = w * v.x + y * v.z - z * v.y
    val iy = w * v.y + z * v.x - x * v.z
    val iz = w * v.z + x * v.y - y * v.x
    val iw = -x * v.x - y * v.y - z * v.z

    // calculate result * inverse quat
    val _x = ix * w - iw * x - iy * z + iz * y
    val _y = iy * w - iw * y - iz * x + ix * z
    val _z = iz * w - iw * z - ix * y + iy * x

    return v.create(_x,_y,_z,w) as Vector4<T, *>
}